#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include <string>

::std::string turtle_name{};

void callback(turtlesim::PoseConstPtr const& msg) noexcept {
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char **argv) noexcept {
    ros::init(argc, argv, "tfbroadcaster");
    ros::NodeHandle node_handle;

    if (!ros::isInitialized()) {
        return 1;
    }

    if (argc != 2) {
        ROS_ERROR("Usage: %s <topic>", argv[0]);
        return 1;
    }

    turtle_name = argv[1];

    ros::Subscriber sub = node_handle.subscribe(turtle_name+"/pose", 10, callback);
    ros::spin();

    return 0;
}
